/* main.c */
#include "app_inc.h"

/*******************************************************************************
 * Definitions
 ******************************************************************************/
#define APP_CFG_ENABLE_PID   0  /* PASS. */

/*******************************************************************************
 * Variables
 ******************************************************************************/
char gAppPrintfBuffer[256]; /* OLED显示屏打印输出的格式化缓冲区 */
uint32_t gAppServoDuty;     /* 当前舵机的控制输出占空比，对应舵机转向角度 */
int32_t gAppMotorLDuty;     /* 当前左驱动电机的控制PWM输出占空比，对应电机转速 */
bool bAppMotorLForward;     /* 当前左驱动电机的控制方向 */
int32_t gAppEncLSpeed;      /* 当前左电机编码器测速值 */
int32_t gAppMotorRDuty;     /* 当前右驱动电机的控制PWM输出占空比，对应电机转速 */
bool bAppMotorRForward;     /* 当前右驱动电机的控制方向 */
int32_t gAppEncRSpeed;      /* 当前右电机编码器测速值 */

PID_Handle_T gAppMotorLPidStruct;

/*******************************************************************************
 * Prototypes
 ******************************************************************************/
void App_BootClockRUN(void);
void App_Tick_PeriodicalCallback(void);

void App_GotoEmptyLoadTest(void);
void App_GotoPIDTest(void);

/*******************************************************************************
 * Code
 ******************************************************************************/
/*!
 * @brief Main function
 */
int main(void)
{
    /* Init board hardware. */
    BOARD_InitBootPins();
    BOARD_BootClockRUN();
    App_BootClockRUN(); /* Enable the clocks additionally not in BOARD_BootClockRUN(). */

    /* Initialize debug console. */
    BOARD_InitDebugConsole(); /* Flexcomm0 - UART0, 115200. */

    PRINTF("\r\nLPCSmartCar AllInOne.\r\n");
    PRINTF("build time: %s, %s\r\n", __TIME__, __DATE__);

    /* OLED显示屏 */
    OLED_Init();
    OLED_PrintReset();
    OLED_PrintStr("LPCSmartCar\r\n");

    /* 按键与延时组件 */
    Tick_Init(200UL); /* 5ms per tick. */
    Tick_InstallPeriodicalCallback(4U, App_Tick_PeriodicalCallback); /* 20ms周期 */

    /* 初始化前进电机控制 */
    OLED_PrintStr("Motor L ...\r\n");
    MotorL_InitDriver();
    OLED_PrintStr("Enc L ...\r\n");
    MotorL_InitEncoder();

    PID_Init(&gAppMotorLPidStruct, 11.5f, 1.0f, 0.0f);

    gAppMotorLDuty = 0;
    bAppMotorLForward = true;
    
    while (1)
    {
        GETCHAR();

#if APP_CFG_ENABLE_PID
        PID_SetTargetValue(&gAppMotorLPidStruct, gAppMotorLDuty);
#else
        MotorL_SetSpeed(gAppMotorLDuty);
#endif /* APP_CFG_ENABLE_PID */



        PRINTF("gAppMotorLDuty: %d\r\n", gAppMotorLDuty);

        Tick_DelayBlocking(200U); /* 等待2s速度变量更新之后读取稳定的速度值 */
        gAppEncLSpeed = MotorL_GetSpeed();
        PRINTF("gAppEncLSpeed: %d\r\n", gAppEncLSpeed);

        if (bAppMotorLForward)
        {
            gAppMotorLDuty++;
            if (gAppMotorLDuty >= MOTOR_L_SPEED_PWM_DUTY_LIMIT)
            {
                bAppMotorLForward = false;
            }
        }
        else
        {
            gAppMotorLDuty--;
            if (gAppMotorLDuty <= -MOTOR_L_SPEED_PWM_DUTY_LIMIT)
            {
                bAppMotorLForward = true;
            }
        }
    }

    OLED_PrintStr("Game Over.\r\n");
    PRINTF("Game Over.\r\n");
    while (1)
    {
    }
}

/* 由Systick周期调用的回调函数 */
void App_Tick_PeriodicalCallback(void)
{
    MotorL_SetSpeed((int32_t)PID_GetAbsOutput(&gAppMotorLPidStruct, MotorL_GetSpeed()));
}


/* 启用在BOARD_BootClockRUN()函数中未启用的时钟
* 这些模块多半也是自定义驱动。SDK提供的驱动内部会开关外设访问时钟
*/
void App_BootClockRUN(void)
{
    CLOCK_EnableClock(kCLOCK_Gpio0);
    CLOCK_EnableClock(kCLOCK_Gpio1);
    CLOCK_EnableClock(kCLOCK_InputMux);
    CLOCK_EnableClock(kCLOCK_Sct0);
    CLOCK_EnableClock(kCLOCK_Dma);
}

/* EOF. */
